#include "rotate.h"
#include "C_code_Log.h"
#include "control.h"
#include "gyro.h"
#include "motor.h"
#include "pid.h"
#include "camera.h"
#include "mydelay.h"
#include "stdlib.h"
static PIDTypeDef gl_sensorPID;  // 传感器PID
static uint8_t rotAngleBase(float angle, Direction direction, int pidLim,
                            int pidP, int pidI, int pidD, uint16_t threshold, uint8_t adjustmentNum,
                            int16_t leftInitSpeed,int16_t rightInitSpeed);
uint8_t rotAngle(float angle) {
  LOG_INFO("rotAngle:%2f", angle);
  return 1;
}

struct RotateInfo
{
    float angle;
    Direction direction;
    int8_t turnNum;
}rotateInfo;

/**
 * @brief 计算偏航角
 * 
 * @param curYaw 当前偏航角，-180~180
 * @param angle 要旋转的角度，从车顶往下看，逆时针为正，顺时针为负,范围-360~360
 * @return float 旋转后的偏航角,-180~180
 */
float calculateYawAngle(float curYaw, float angle)
{
    float yaw = curYaw + angle;
    if(yaw > 180)
    {
        yaw = yaw - 360;
    }
    else if(yaw < -180)
    {
        yaw = yaw + 360;
    }
    return yaw;
}

float errCalculate(float curYaw, float targetYaw,Direction direction)
{
    float err = 0;
    if(direction == Left)
    {
        err = targetYaw - curYaw;
    }
    else if(direction == Right)
    {
        err = curYaw - targetYaw;
    }
    if(err > 180)
    {
        err = err - 360;
    }
    else if(err < -180)
    {
        err = err + 360;
    }
    return err;
}
/**
 * @brief 求偏航角的角度数组的平均值
 * 
 * @param angle 角度数组，每个值的取值为-180~180
 * @param size 数组大小,最大为100
 * @param mode 0:正常平均值 1:去除最大最小值的平均值
 * @return float 
 */
float yawAngleAverage(float angle[],uint8_t size,uint8_t mode){
  if(size>100||size == 0) return 0;
    float sum = 0;
    float baseAngle = angle[0];
    float err[100];
    for(uint8_t i = 0;i<size;i++){
        err[i] = errCalculate(baseAngle,angle[i],Left);
        sum += err[i];
    }
    if(mode == 1){
        float max = err[0];
        float min = err[0];
        for(uint8_t i = 0;i<size;i++){
            if(err[i]>max) max = err[i];
            if(err[i]<min) min = err[i];
        }
        sum = sum - max - min;
        sum = sum/(size-2);
    }
    else{
        sum = sum/size;
    }
  return calculateYawAngle(baseAngle,sum);
}
/**
 * @brief 原地旋转
 * 
 * @param angle 旋转角度
 * @param direction 旋转方向
 * @return  旋转完成返回1，否则返回0
 */
uint8_t rotAngleCommon(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    uint8_t result = rotAngleBase(angle,direction,2500,30,5,15,15,5,400,400);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
uint8_t Test_rotAngle(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    extern int gl_value1,gl_value2,gl_value3,gl_value4,gl_value5,gl_value6;
    uint8_t result = rotAngleBase(angle,direction,gl_value1,gl_value2,gl_value3,gl_value4,0,2,gl_value5,gl_value6);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
/**
 * @brief 小角度原地旋转，未调参
 * 
 * @param angle 旋转角度
 * @param direction 旋转方向
 * @return  旋转完成返回1，否则返回0
 */
static uint8_t rotAngle_Little(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    uint8_t result = rotAngleBase(angle,direction,2500,25,0,12,0,0,600,600);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
/**
 * @brief 中角度(60~90)原地旋转，未调参
 * 
 * @param angle 旋转角度
 * @param direction 旋转方向
 * @return  旋转完成返回1，否则返回0
 */
static uint8_t rotAngle_Middle(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    uint8_t result = rotAngleBase(angle,direction,2500,26,5,12,10,2,750,750);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
/**
 * @brief 平台上转弯
 * 
 * @param angle 旋转角度
 * @param direction 旋转方向
 * @note 想转180要输入175度
 * @return  旋转完成返回1，否则返回0
 */
uint8_t rotAnglePlatform(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    uint8_t result = rotAngleBase(angle,direction,2500,26,5,9,10,2,600,600);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
/**
 * @brief 平台上转弯
 * 
 * @param angle 旋转角度
 * @param direction 旋转方向
 * @note 想转180要输入175度
 * @return  旋转完成返回1，否则返回0
 */
uint8_t rotAnglePlatform_ONE(float angle,Direction direction){
    static bool (*save)(void) = NULL;
    if(gl_controlCenter.refresh != refresh_Time)
    {
        save = gl_controlCenter.refresh;
        //把刷新方法改为20ms刷新一次
        setRefreshFunction(&gl_controlCenter,refresh_Time);
    }
    uint8_t result = rotAngleBase(angle,direction,2500,26,5,9,10,2,600,600);
    if(result == 1&& save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
    }
    return result;
}
/**
 * @brief 任意角度旋转基础函数
 * 
 * @param angle 角度值，为正
 * @param direction 旋转方向Left/Right
 * @param pidLim pid限幅
 * @param pidP pid比例
 * @param pidI pid积分
 * @param pidD pid微分
 * @param threshold 积分分离阈值
 * @param adjustmentNum 稳定计数器
 * @param leftInitSpeed 左电机最小速度
 * @param rightInitSpeed 右电机最小速度
 * @return uint8_t 1表示旋转完成，0表示未完成
 * @note 根据陀螺仪的数据，计算当前的偏航角，
 * 然后根据目标角度和当前角度计算出偏差，然后根据偏差调用PID算法计算出电机的速度
 * @note 调节周期为50ms，即一直调用最快也是50ms调节一次电机速度
 * @note 旋转前和完成后，都会调用motorStop()停止电机
 * @note 新增积分分离PID，当误差绝对值大于阈值时，清零积分的累计误差,当阈值为0时，积分项不起作用
 */
static uint8_t rotAngleBase(float angle,Direction direction,
                            int pidLim,int pidP,int pidI,int pidD,uint16_t threshold,uint8_t adjustmentNum,
                            int16_t leftInitSpeed,int16_t rightInitSpeed){
    static uint8_t flag = 0;
    static float oldYaw = 0;
    static uint32_t runTargetTime;
    //在角度误差处于允许范围内后，如果马上停止电机，由于惯性,车会继续旋转一段距离，所以需要一个稳定计数器，用于回调
    static uint8_t stable;
    if(flag == 0){//第一次进入
          //打开陀螺仪
        
        //等待陀螺仪获取数据，如果想用旧的数据，那就不用这句，直接gyro_getYaw()就行
        //while(gyro_getData() != 1);
        float nowYaw = gyro_getYaw();
        if(direction == Left) rotateInfo.angle = angle+nowYaw;
        else if(direction == Right) rotateInfo.angle = nowYaw-angle;
        rotateInfo.direction = direction;
        rotateInfo.turnNum = 0;
        flag = 1;
        oldYaw = nowYaw;
        stable = 0;
        PID_Init(&gl_sensorPID, rotateInfo.angle, pidLim, pidP, pidI, pidD);
        motorStop();
      
        runTargetTime = getGlobalTime();
        //LOG_INFO("nowAngle: %.2f",nowYaw);
        //LOG_INFO("TargetAngle: %.2f",rotateInfo.angle);
    }
    if (runTargetTime <= getGlobalTime()) {
        runTargetTime+=50;
      float nowYaw = gyro_getYaw();
        //LOG_INFO("nowYaw: %.2f",nowYaw);
      if (nowYaw - oldYaw > 300)
        {rotateInfo.turnNum--;LOG_INFO("turnNum: %d",rotateInfo.turnNum);}
      else if (nowYaw - oldYaw < -300)
        {rotateInfo.turnNum++;LOG_INFO("turnNum: %d",rotateInfo.turnNum);}
        oldYaw = nowYaw;
      float error = nowYaw + rotateInfo.turnNum * 360 - rotateInfo.angle;
      //LOG_INFO("error: %.2f",error);
      if (error < 6 && error > -6) {
        stable++;
        if (stable > adjustmentNum) {
          motorStop();
          flag = 0;
          return 1;
        }
      }
      int16_t speed =
          positionPiD_IntegralSeparate(&gl_sensorPID, nowYaw + rotateInfo.turnNum * 360, threshold);
      if (speed > 0)  // 需要左转
        speedAdjustment(-leftInitSpeed - speed, rightInitSpeed + speed);
      else if (speed < 0)  // 需要右转
        speedAdjustment(leftInitSpeed - speed, -rightInitSpeed + speed);
    }

    return 0;
}

/**
 * @brief 抓线转弯
 * 
 * @note 需要在节点前稳定下来后调用
 * @return  归正完成返回1，否则返回0
 */
uint8_t rotate_trace(Direction dir) {
  static uint8_t flag = 0;
  static uint8_t stableNum = 0;
  int rank;
  if (flag == 0) {
    // 进入跟踪模式
    if(dir==Left)
      camera_SendCommand(TRACE_L_CMD_FIND_LINE);
    else 
      camera_SendCommand(TRACE_R_CMD_FIND_LINE);
    stableNum = 0;
    flag = 1;
  }
  if (flag == 1) {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 2000, 120, 0, 35);
    flag = 2;
  }
  if (flag >= 2) {
    // 开始跟踪
    int difSpeed;
    rank = getLineRank();
    difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
    positionSpeedOut(1200, 1200, difSpeed);  // 位置式改变电机速度
   
	}
  if(flag==2&&myDelay((uint32_t)rotate_trace,600)==true){
    flag = 3;
  }
  if(flag == 3){
     const int ALLOW_ERR = 7;
    if (inRange(rank, 80+ALLOW_ERR, 80-ALLOW_ERR)) {
      stableNum++;
    } else {
      stableNum = 0;
    }
    if (stableNum > 5) {
      camera_SendCommand(CAMERA_CMD_FIND_LINE);
      flag = 0;
      return 1;
    }
  }
    return 0;
}
#include "math.h"
#define TRANSFORM_RADIAN(Angle) (Angle/180.0*3.1415926)
/**
 * @brief 以弧线的方式快速转弯
 * 
 * @param angle 非180度的角度,左转为正，右转为负
 * @param distance 转弯时距离节点的距离,单位mm
 * @param speed 平均速度，单位mm/s
 * @return uint8_t 
 * @note 用编码器做速度环的控制，通过控制两个电机的速度来控制转弯速度和转弯半径
 */
uint8_t rotFast(int16_t angle,uint16_t distance,float speed,int16_t exitAngle){
  if(angle == 180 || angle == 0) return 1;
  static enum{START,ROTATE} Flag = START;
  static int startAngle = 0;
  static bool (*save)(void) = NULL; 
  if (Flag == START) {
    startAngle = gyro_getYaw();
    float radius = distance / tan(TRANSFORM_RADIAN(angle) / 2);
    Flag = ROTATE;
    float difference = speed / radius * CAR_WHEEL_LR_DISTANCE_VIRTUAL;
    float rightSpeed = (speed*2 + difference) / 2;  // 单位为mm/s
    float leftSpeed = (speed*2 - difference) / 2;   // 单位为mm/s

    gl_motor.leftSpeed =
        leftSpeed * ENCODE_OF_ONE_mm/10;  // 单位为0.1s的脉冲数
    gl_motor.rightSpeed =
        rightSpeed  * ENCODE_OF_ONE_mm/10;  // 单位为0.1s的脉冲数

    //改变电机的运行模式为固定速度模式
    motorRunModeChange(MOTOR_MODE_SPEED);

    //把刷新方法改为固定时间刷新一次
    save = gl_controlCenter.refresh;
    setRefreshFunction(&gl_controlCenter,refresh_Time);
  } else if (Flag == ROTATE) {
    //退出条件
    if (abs16((int16_t)errCalculate(gyro_getYaw(), startAngle, Left)) >
        abs16(exitAngle)) {
      motorRunModeChange(MOTOR_MODE_Direct);
      Flag = START;
      HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
      if(save!=NULL){
        setRefreshFunction(&gl_controlCenter,save);
      }
      return 1;
    }
  }
  return 0;
}
/**
 * @brief 以弧线的方式快速转弯
 * 
 * @param angle 非180度的角度，左转为正，右转为负
 * @param distance 转弯时距离节点的距离,单位mm
 * @param speed 平均速度，单位mm/s
 * @return uint8_t 
 * @note 通过编码器和陀螺仪实现惯性导航，然后就像巡线一样走
 * @note 调节周期为50ms，即一直调用最快也是50ms调节一次电机速度，与编码器保持一致
 * @warning 没有测试，因为在转弯的过程中轮子会出现打滑的情况，所以惯性导航的距离获取是有问题的。
 */
/*uint8_t rotFast_2(int16_t angle,uint16_t distance,uint16_t pwm){
  static enum{START,ROTATE} flag = START;
  static struct RotateInfo{
    float startAngle;
    float lastAngle;
    float radius;
    float lastX,lastY;
    bool (*save)(void);
    uint32_t lastDistance;
    uint32_t targetRefreshTime;
    uint16_t initLeftSpeed,initRightSpeed;//初始速度,单位：脉冲/0.1s
  }*rotateInfo;
  if(angle == 180 || angle == 0) return 1;
  if(flag==START){
    rotateInfo = (struct RotateInfo*)malloc(sizeof(struct RotateInfo));
    if(rotateInfo == NULL) {
      LOG_ERROR("malloc error");
      while(1);
    }
    rotateInfo->save = gl_controlCenter.refresh;
    setRefreshFunction(&gl_controlCenter,refresh_Time);
    flag = ROTATE;
    rotateInfo->startAngle = gyro_getYaw();
    rotateInfo->lastAngle = rotateInfo->startAngle;
    rotateInfo->radius = distance / tan(TRANSFORM_RADIAN(angle) / 2);
    rotateInfo->lastX = 0;
    rotateInfo->lastY = 0;
    rotateInfo->lastDistance = gl_motor.encoderDistance;
    PID_Init(&gl_sensorPID, 80, 1200, 50, 0, 5);
      float differencePWM = pwm / rotateInfo->radius * CAR_WHEEL_LR_DISTANCE_VIRTUAL;
     rotateInfo->initRightSpeed= (pwm*2 + differencePWM) / 2;  // 单位为脉冲/0.1s
      rotateInfo->initLeftSpeed = (pwm*2 - differencePWM) / 2;   // 单位为脉冲/0.1s

  }
  if(getGlobalTime()>=rotateInfo->targetRefreshTime){
    rotateInfo->targetRefreshTime+=50;
  }else{
    return 0;
  }
  if(flag == ROTATE){
    //算出当前角度与上一个角度的平均值与初始朝向角的差值
    float nowAngle = gyro_getYaw();
    float angleArr[2] = {rotateInfo->lastAngle,nowAngle};
    float angleAverage=yawAngleAverage(angleArr,2,0);
    rotateInfo->lastAngle = nowAngle;
    //算出当前角度与上一个角度的平均值与初始朝向角的差值
    float angleAverageDifference = errCalculate(angleAverage,rotateInfo->startAngle,Left);
    float angleAverageRad = TRANSFORM_RADIAN(angleAverageDifference);
    //算出当前角度与初始朝向角的差值
    float angleDifference = errCalculate(nowAngle,rotateInfo->startAngle,Left);
    float angleRad = TRANSFORM_RADIAN(angleDifference);
    //更新坐标，单位mm，原理为把走过的距离当成直线，走的角度为当前角度与上一个角度的平均值
    float distanceDifference = (gl_motor.encoderDistance - rotateInfo->lastDistance)/ENCODE_OF_ONE_mm;
    float dX= distanceDifference*sin(angleAverageRad);
    float dY= distanceDifference*cos(angleAverageRad);
    rotateInfo->lastX += dX;
    rotateInfo->lastY += dY;
    //用车头的前方一段距离的点与圆心的距离来计算当前的角度
    const float distanceFront = 50;
    dX = distanceFront*sin(angleRad);
    dY = distanceFront*cos(angleRad);
    float frontX = rotateInfo->lastX + dX;
    float frontY = rotateInfo->lastY + dY;
    int rank = sqrt((frontX + rotateInfo->radius) *(frontX + rotateInfo->radius) +
     frontY* frontY)-rotateInfo->radius;
    if(angle>0){//左转的话rank为负
      rank = -rank;
    }
    int difSpeed = positionPIDCalc(&gl_sensorPID, rank);
    positionSpeedOut(rotateInfo->initLeftSpeed, rotateInfo->initRightSpeed, difSpeed);
    if(abs16((int16_t)angleDifference) > abs16(angle)){
      if(rotateInfo->save!=NULL){
        setRefreshFunction(&gl_controlCenter,rotateInfo->save);
      }
      flag = START;
      return 1;
    }
  }
  return 0;
  
}*/

/**
 * @brief 以弧线的方式快速转弯,方法3
 * 
 * @param angle 非180度的角度，左转为正，右转为负
 * @param distance 转弯时距离节点的距离,单位mm
 * @param PWM 期望的电机PWM的平均值
 * @param exitAngle 退出角度，用于停止条件的判断，因为惯性，所以要提前结束，不能减小angle的值，因为会影响转弯半径的计算
 * @note 与方法1的区别：不使用速度环，直接控制电机的PWM，电机的PWM根据经验值直接给定
 * @return uint8_t 
 * @note 效果没有方法1好，故不使用
 */
// uint8_t rotFast_3(int16_t angle,uint16_t distance,int16_t PWM,int16_t exitAngle){
//    if(angle == 180 || angle == 0) return 1;
//   static enum{START,ROTATE} Flag = START;
//   static int startAngle = 0;
//   static bool (*save)(void) = NULL;
//   if (Flag == START) {
//     Flag = ROTATE;
//     startAngle = gyro_getYaw();
//     float radius = distance / tan(TRANSFORM_RADIAN(angle) / 2);
//     // float speed = (getLeftMotorSpeedByPWM(gl_motor.rightPWM)+getLeftMotorSpeedByPWM(gl_motor.leftPWM))*10/ENCODE_OF_ONE_mm/2;
//     float speed = getLeftMotorSpeedByPWM(PWM)*10/ENCODE_OF_ONE_mm;
//     float difference = speed / radius * CAR_WHEEL_LR_DISTANCE_VIRTUAL;
//     float rightSpeed = (speed*2 + difference) / 2;  // 单位为mm/s
//     float leftSpeed = (speed*2 - difference) / 2;   // 单位为mm/s

//     int16_t EncoderLeftSpeed =
//         leftSpeed * ENCODE_OF_ONE_mm/10;  // 单位为0.1s的脉冲数
//     int16_t EncoderRightSpeed =
//         rightSpeed  * ENCODE_OF_ONE_mm/10;  // 单位为0.1s的脉冲数
//     int16_t leftPWM = getLeftMotorPWMBySpeed(EncoderLeftSpeed);
//     int16_t rightPWM = getRightMotorPWMBySpeed(EncoderRightSpeed);
//     speedAdjustment(leftPWM,rightPWM);

//     //把刷新方法改为固定时间刷新一次
//     save = gl_controlCenter.refresh;
//     setRefreshFunction(&gl_controlCenter,refresh_Time);
//   }
//   else if (Flag == ROTATE) {
//     //退出条件
//     if (abs16((int16_t)errCalculate(gyro_getYaw(), startAngle, Left)) >
//         abs16(exitAngle)) {
//       Flag = START;
//       HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
//       if(save!=NULL){
//         setRefreshFunction(&gl_controlCenter,save);
//       }
//       return 1;
//     }
//   }
//   return 0;
// }

/**
 * @brief 抓线转弯
 * 
 * @note 需要在节点前稳定下来后调用
 * @return  归正完成返回1，否则返回0
 */
uint8_t rotate_trace_time(Direction dir) {
  static uint8_t flag = 0;
  static uint8_t stableNum = 0;
  int rank;
  if (flag == 0) {
    // 进入跟踪模式
    if(dir==Left)
      camera_SendCommand(TRACE_L_CMD_FIND_LINE);
    else 
      camera_SendCommand(TRACE_R_CMD_FIND_LINE);
    stableNum = 0;
    flag = 1;
  }
  if (flag == 1) {
    // 对速度PID的参数进行初始化设置
    PID_Init(&gl_sensorPID, 80, 1200, 75, 0, 35);
    flag = 2;
  }
  if (flag >= 2) {
    // 开始跟踪
    int difSpeed;
    rank = getLineRank();
    difSpeed = positionPIDCalc(&gl_sensorPID, rank);  // 速度位置式PID输出
    positionSpeedOut(1000, 1000, difSpeed);  // 位置式改变电机速度
   
	}
  if(flag==2&&myDelay((uint32_t)rotate_trace,500)==true){
    flag = 0;
    return 1;
  }
    return 0;
}
/**
 * @brief 以恒定速度过弯
 * 
 * @param angle 非180度的角度，左转为正，右转为负
 * @param distance 转弯时距离节点的距离,单位mm
 * @param exitAngle 退出角度，用于停止条件的判断，因为惯性，所以要提前结束，不能减小angle的值，因为会影响转弯半径的计算
 * @return uint8_t 
 * @note 使用rotFast方法，速度为调用时的速度
 */
static uint8_t rotFastAutoSpeed(int16_t angle,uint16_t distance,int16_t exitAngle){
  static uint8_t flag = 0;
  static float speed;//单位mm/s
  if(flag == 0){
    flag = 1;
    if (abs16(angle) > 90) {  // 大弯，要减速
      speed = 500/0.8;
    } else {
      // 获取当前速度
      // speed = (gl_motor.encoderLeftFront +
      // gl_motor.encoderRightFront)/2/ENCODE_OF_ONE_mm*10;
      // 使用当前PWM值估算速度，可以避免车还在减速，速度没有降下来而导致的过弯速度过快
#ifndef FOUR_ENCODER
      speed = (getRightMotorSpeedByPWM(1500) + getLeftMotorSpeedByPWM(1500)) *10 / ENCODE_OF_ONE_mm / 2;
#else
      speed = (getLeftFrontMotorSpeedByPWM(getLeftMotorPWM()) + 
      getRightFrontMotorSpeedByPWM(getRightMotorPWM()) +
      getLeftBackMotorSpeedByPWM(getLeftMotorPWM()) +
      getRightBackMotorSpeedByPWM(getRightMotorPWM())
      ) *10 / ENCODE_OF_ONE_mm / 4;
#endif
    }
  }
  if(flag == 1){
    if(rotFast(angle,distance,0.8*speed,exitAngle)){
      flag = 0;
      return 1;
    }
  }
	return 0;
}
/// @brief 旋转任务回调函数
/// @param control 控制台
/// @note 在完成旋转任务后调用，将旋转任务状态设置为完成，巡线任务状态置为完成，将更新状态置为准备就绪，表示路段已经完成
static void rotateCallback(ControlCenterTypeDef *control)
{
    control->runState->RotateState = MY_EXIT;
    //转弯的时候不需要巡线，所以停车完后就把巡线任务状态置为完成，2024/6/2 by Sevenfite
    //control->runState->F_LineState = MY_EXIT;
    control->update=MY_READY;
}

/// @brief 旋转任务处理函数
/// @param control 控制台
/// @note 在旋转任务状态为准备就绪时调用，根据旋转任务的类型执行不同的旋转任务
/// @note 旋转任务完成后调用旋转任务回调函数
void rotateHandle_Task(ControlCenterTypeDef *control){
    int result = 0;
    // 如果旋转任务状态为准备就绪
    if (control->runState->RotateState == MY_READY) {
      // 根据旋转任务的类型执行不同的旋转任务，sectionInfo是当前路段的信息的指针，指向地图
      switch (control->sectionInfo->rotateWay) {
        case HL_15:
          result = rotAngle_Little(15, Left);
          break;
        case HL_30:
          result = rotAngle_Little(30, Left);
          break;
        case HL_45:
          result = rotAngle_Little(45, Left);
          break;
        case HL_60:
          result = rotAngle_Middle(60, Left);
          break;
        case HL_75:
          result = rotAngle_Middle(75, Left);
          break;
        case HL_90:
          result = rotAngle_Middle(90, Left);
          break;
        case HL_100:
          result = rotAngle_Middle(100, Left);
          break;  
        case HL_135:
          result = rotAngleCommon(135, Left);
          break;
        case HL_140:
          result = rotAngleCommon(140, Left);
          break;
        case HL_180:
          result = rotAngleCommon(180, Left);
          break;
        case HR_30:
          result = rotAngle_Little(30, Right);
          break;
        case HR_45:
          result = rotAngle_Little(45, Right);
          break;
        case HR_60:
          result = rotAngle_Middle(60, Right);
          break;
        case HR_70:
          result = rotAngle_Middle(70, Right);
          break;
        case HR_75:
          result = rotAngle_Middle(75, Right);
          break;
        case HR_90:
          result = rotAngle_Middle(90, Right);
          break;
        case HR_135:
          result = rotAngleCommon(135, Right);
          break;
        case HR_150:
          result = rotAngleCommon(150, Right);
          break;
        case H_trace_r:
          result = rotate_trace(Right);
          break;  
        case H_trace_l:
          result = rotate_trace(Left);
          break;  
#define FIND_NODE_DISTANCE 120//转弯时距离节点的距离，（这里为中距离找点的距离）单位mm
#define FIND_NODE_DISTANCE_REMOTE 270//转弯时距离节点的距离，（这里为远距离找点的距离,转90以上的弯用）单位mm
        case HL_ARC_15:
          result = rotFastAutoSpeed(15, 100, 15);
          break;
        case HR_ARC_15:
          result = rotFastAutoSpeed(-15, 100, 15);
          break;
        case HL_ARC_20:
          result = rotFastAutoSpeed(20, 100, 20);
          break;
        case HR_ARC_20:
          result = rotFastAutoSpeed(-20, 100, 20);
          break;
        case HL_ARC_30:
          result = rotFastAutoSpeed(30, 100, 30);
          break;
        case HR_ARC_30:
          result = rotFastAutoSpeed(-30, 100, 30);
          break;
        case HL_ARC_45:
          result = rotFastAutoSpeed(45, 100, 45);
          break;
        case HR_ARC_45:
          result = rotFastAutoSpeed(-45, 100, 45);
          break;
        case HL_ARC_90:
          result = rotFastAutoSpeed(90, 110,60);
          break;  
        case HR_ARC_90:
          result = rotFastAutoSpeed(-90, FIND_NODE_DISTANCE,60);
          break;
        case HL_ARC_110:
          result = rotFastAutoSpeed(110, 130, 75);
          break;
        case HL_ARC_120:
          result = rotFastAutoSpeed(120,270,90);
          break;
        case HL_ARC_130:
          result = rotFastAutoSpeed(130,250,100);
          break;
        case HR_ARC_120:
          result = rotFastAutoSpeed(-120,180,100);
          break;
        case HR_ARC_150:
          result = rotFastAutoSpeed(-140,FIND_NODE_DISTANCE_REMOTE,105);
          break;
        case HR_ARC_160:
          result = rotFastAutoSpeed(-160,FIND_NODE_DISTANCE_REMOTE,100);
          break;
        case HL_ARC_150:
          result = rotFastAutoSpeed(150,320,110);
          break;
        default:
          result = 1;
          break;
      }
      if (result == 1) {
        rotateCallback(control);
      }
    }
}